/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package org.usfirst.team3132.demo;


import edu.wpi.first.wpilibj.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot3132 extends SimpleRobot {

    final int leftMotorChannel = 1;
    final int rightMotorChannel = 2;
    float motorSpeed = 0;
    float motorRotation = 0;
    
    //Starting stick values
    double leftStickInitialX;
    double leftStickInitialY;
    double rightStickInitialX;
    double rightStickInitialY;
    
    //Min stick values
    double leftStickMinX = 0;
    double leftStickMinY = 0;
    double rightStickMinX = 0;
    double rightStickMinY = 0;
    
    //Max stick values
    double leftStickMaxX = 0;
    double leftStickMaxY = 0;
    double rightStickMaxX = 0;
    double rightStickMaxY = 0;
    
    //Encoder int values
    int leftEncoderRPM = 0;
    int rightEncoderRPM = 0;
    double gyroAngle = 0;
    double accelerometerValue = 0;
    
    // RobotDrive assumes that it's using Jaguars (we are).
    RobotDrive drive = new RobotDrive(leftMotorChannel, rightMotorChannel);
    Joystick leftStick = new Joystick(1);
    Joystick rightStick = new Joystick(2);
    Encoder leftEncoder = new Encoder(1,2);
    Encoder rightEncoder = new Encoder(3,4);
    
    Gyro gyro = new Gyro(1);
    //Accelerometer accelerometer = new Accelerometer();
    DriverStationLCD driverStationLCD = DriverStationLCD.getInstance();
    
    public Robot3132() {
        System.out.println("It's alive!");
        // Disable watchdog.
        drive.setSafetyEnabled(false);
    }
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        System.out.println("autonomus mode");
        for (int i = 0; i < 4; i++) {
            drive.drive(0.5, 0.0);  // drive 50% forward speed with 0% turn
            Timer.delay(2.0);  // wait 2 seconds
            drive.drive(0.0, 0.75); // drive 0% forward with 75% turn
            Timer.delay(0.75); // wait for the 90 degree turn to complete
        }
        drive.drive(0.0, 0.0);  // drive 0% forward with 0% turn (stop)
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        System.out.println("controller mode");
        startEncoders();    
        gyro.reset();
        while (isOperatorControl() && isEnabled()) // loop during enabled teleop mode
        {
            configureLeftStick();
            configureRightStick();
            
            drive.tankDrive(leftStick, rightStick);

            leftEncoderRPM = (int)leftEncoder.getRate();
            rightEncoderRPM = (int)rightEncoder.getRate();
            gyroAngle = gyro.getAngle();
            System.out.println(leftEncoderRPM);
            System.out.println(rightEncoderRPM);
            //accelerometerValue = accelerometer.getAcceleration();
            //System.out.println(gyroAngle);
            //System.out.println(accelerometerValue);
            
            //Outputs encoder rate to Driver Station
            //clearScreen();
            //driverStationLCD.println(DriverStationLCD.Line.kMain6, 1, "left RPM "+leftEncoderRPM+"\n");
            //driverStationLCD.println(DriverStationLCD.Line.kUser2, 1, "right RPM "+ rightEncoderRPM+"\n");
            //driverStationLCD.println(DriverStationLCD.Line.kUser2, 1, String.valueOf(rightEncoder.getRate()));
            //driverStationLCD.println(DriverStationLCD.Line.kUser2, 1, "01234567890123456789012345678901234567890123456789");
            
            driverStationLCD.updateLCD();
            Timer.delay(0.5);
        }
        stopEncoders();
    }
    
    private void clearScreen()
    {
        driverStationLCD.println(DriverStationLCD.Line.kMain6, 1, "\n\n\n\n\n\n\n\n\n\n\n\n");
    }
    
    private void startEncoders()
    {
        leftEncoder.start();
        rightEncoder.start();
    }
    
    private void stopEncoders()
    {
        leftEncoder.stop();
        leftEncoder.reset();
        rightEncoder.stop();
        rightEncoder.reset();
    }
    
    private void configureLeftStick()
    {
        if (leftStick.getTrigger()) 
        {
            leftStickInitialX = leftStickMinX = leftStickMaxX = leftStick.getX();
            leftStickInitialY = leftStickMinY = leftStickMaxY = leftStick.getY();
            while (leftStick.getTrigger())
            {
                if (leftStick.getX() > leftStickInitialX && leftStick.getX() > leftStickMaxX) leftStickMaxX = leftStick.getX();
                else if (leftStick.getX() < leftStickInitialY && leftStick.getX() < leftStickMinX) leftStickMinX = leftStick.getX();
                if (leftStick.getY() > leftStickInitialX && leftStick.getY() > leftStickMaxY) leftStickMaxY = leftStick.getY();
                else if (leftStick.getY() < leftStickInitialY && leftStick.getY() < leftStickMinY) leftStickMinY = leftStick.getY();
            }
            System.out.println(leftStickMinX + " Min X");
            System.out.println(leftStickMaxX + " Max X");
            System.out.println(leftStickMinY + " Min Y");
            System.out.println(leftStickMaxY + " Max Y");
        }
    }
    
    private void configureRightStick()
    {  
        if (rightStick.getTrigger())
        {               
            rightStickInitialX = rightStickMinX = rightStickMaxX = rightStick.getX();
            rightStickInitialY = rightStickMinY = rightStickMaxY = rightStick.getY();
            while (rightStick.getTrigger())
            {
                if (rightStick.getX() > rightStickInitialX && rightStick.getX() > rightStickMaxX) rightStickMaxX = rightStick.getX();
                else if (rightStick.getX() < rightStickInitialX && rightStick.getX() < rightStickMinX) rightStickMinX = rightStick.getX();
                if (rightStick.getY() > rightStickInitialY && rightStick.getY() > rightStickMaxY) rightStickMaxY = rightStick.getY();
                else if (rightStick.getY() < rightStickInitialY && rightStick.getY() < rightStickMinY) rightStickMinY = rightStick.getY();
            }
            System.out.println(rightStickMinX + " Min X");  
            System.out.println(rightStickMaxX + " Max X");
            System.out.println(rightStickMinY + " Min Y");
            System.out.println(rightStickMaxY + " Max Y");
        }
    }
}
